#ifndef CARTOGRAPHER_ADAPTIVE_SCAN_MATCHER_2D_H_
#define CARTOGRAPHER_ADAPTIVE_SCAN_MATCHER_2D_H_

#include "cartographer/mapping/internal/2d/scan_matching/adaptive_matcher.h"

namespace cartographer
{
    class AdaptiveWindow
    {
    public:
        using Ptr = std::shared_ptr<AdaptiveWindow>;

        AdaptiveWindow(const ScanMatchOptions &options);

        void Reset(const int num_rotation, const int num_cand = 0);

        int Resample();

        void SampleCommon();

        void SampleNears();

        void SampleExhaustive();

        void SampleNearsOnlyYaw();

        void UpdateWeightByScore();

        bool ResetSampleOffsetStride();

        void ResetSampleOffsetStride1();

        void NormalizeWeight(std::vector<double> &weights, const double weight_sum);

        void UpdateWeightByExp();

        void ResetLinearBounds(const int num_rotate_scans);

        void DiscretizeScans(std::vector<mapping::scan_matching::DiscreteScan2D> &discrete_scans, const mapping::MapLimits &map_limits,
                             const std::vector<sensor::PointCloud> &scans, const double x, const double y);

        std::vector<sensor::PointCloud> GenerateRotatedScans(const sensor::PointCloud &cloud);

        void UpdateParameter(const float max_range, const double submap_resolution);

        mapping::scan_matching::Candidate2D GetCandidate(int id, int scan_index, int x_offset, int y_offset, double yaw);

        void ScoreCandidates(const mapping::Grid2D &grid, const std::vector<mapping::scan_matching::DiscreteScan2D> &discrete_scans);

        void ScoreCandidates(const mapping::Grid2D &grid, const std::vector<sensor::PointCloud> &rotated_clouds, const double x,
                             const double y);

        void Failed(const transform::Rigid2d &pose);

        int Feedback(const transform::Rigid2d &pose_init, const transform::Rigid2d &pose_opt);

        void UpdateRotateByCandidates();

        void UpdateOffsetByCandidates(std::vector<Eigen::Vector2d> &keeps);

        void UpdateSigma(const Eigen::Vector3d &delta);

        double ComputeWeightRatio(const double x, const double y, const double yaw);

        bool Check();

        void PrintSpan(const char *name);

        void PrintMaxWeightInfo(int maxIdx, double weightMax);

        Eigen::Vector3d sigma_;

        bool inited_ = false;
        int num_scans_;
        int best_idx_ = 0;
        size_t num_candidates = 0;

        double score_max_ = 0;
        double win_linear;
        double win_angular;
        double win_def_linear;
        double win_def_angular;
        double cost_weight_linear = 0;
        double cost_weight_angular = 0;

        int idCnt_ = 0;
        int linear_step_size;
        double resolution_linear;

        int angular_step_size;
        double angular_stride;

        double alpha_slow_ = 0.3;
        double alpha_fast_ = 0.7;
        double weight_slow_ = -1;
        double weight_fast_ = -1;

        Eigen::Vector3d offsetBest;
        Eigen::Vector3d offsetDelta_;
        Eigen::Vector3d offsetExpect;
        Eigen::Vector3i offsetStride;
        std::deque<Eigen::Vector3d> offsetDeltaHis_;

        std::vector<double> weights_;
        std::vector<double> rotations_;
        std::vector<mapping::scan_matching::Candidate2D> candidates;
        std::vector<LinearBounds> linear_bounds;
    };

    class AdaptiveScanMatcher2D : public RealTimeScanMatcher2D
    {
    public:
        explicit AdaptiveScanMatcher2D(const ScanMatchOptions &options);

        AdaptiveScanMatcher2D(const AdaptiveScanMatcher2D &) = delete;
        AdaptiveScanMatcher2D &operator=(const AdaptiveScanMatcher2D &) = delete;

        double Match(const transform::Rigid2d &initial_pose_estimate, const sensor::PointCloud &point_cloud, const mapping::Grid2D &grid,
                     transform::Rigid2d *pose_estimate);

        AdaptiveWindow::Ptr filter_;

    private:
        const ScanMatchOptions options_;
    };
} // namespace cartographer

#endif